/* ========================================
 *
 * Copyright YOUR COMPANY, THE YEAR
 * All Rights Reserved
 * UNPUBLISHED, LICENSED SOFTWARE.
 *
 * CONFIDENTIAL AND PROPRIETARY INFORMATION
 * WHICH IS THE PROPERTY OF your company.
 *
 * ========================================
*/

#include <device.h>

 

//// CONTROL PANEL ////
int stepstate = 0; // Retur værdi fra motoren omkring hvor langt den er nået
float Freq = 100; // max freq for motor
int steps = 2000; // Bruges som indput til hvor mange steps vi vil tage
int position = 0; // Bruges til og holde styr på hvor man steps der er kørt
int spolecase = 0; // Bruges til og holde styr på hvilken spole motoren sidst brugte ved Full step er der 4 kombinationer
int spolecaseHalf = 0; //  Bruges til og holde styr på hvilken spole motoren sidst brugte ved half step er der 8 kombinationer
int mode = 1; // Mode 1 motoren acces op  // Mode 0 motoren starter med den definerede Freq
float stepup = 0; // Bruges i mode 1 hvor motoren starter fra 0 hertz og arbejders sig op til en given Freq
//// CONTROL PANEL ////
int Last = 0; // Holder styr på om der sidst blev kørt frem eller tilbage det er vigtigt da ved 2x frem skal springes 1 spole over
float delay = 0; // gloabal konstant delay som bruges i flere funktioner
int pins[4]  = {CYREG_PRT0_PC0,CYREG_PRT0_PC1,CYREG_PRT0_PC2,CYREG_PRT0_PC3}; // Defineret af de 4 pins første tal er Port benet og andet tal er pin ben
//void turn1stepForw(); // Funktion til Fremad waveform
//void turn1stepRev(); // Funktion til Bagud waveform
int turnWaveForw(int, float, int); // Funktion til Fremad wavestep 50% torque
int turnWaveRev(int, float, int); // Funktion til Fremad wavestep 50% torque
int turnFullForw(int, float, int); // Funktion til Fremad fullstep 100% torque
int turnFullRev(int, float, int); // Funktion til Reverseret fullstep 100% torque
int turnHalfForw(int, float, int); // Funktion til Fremad halfstep ca 70% torque
int turnHalfRev(int, float, int); // Funktion til Reverseret fullstep ca 70% torque



void main()
{
    /* Place your initialization/startup code here (e.g. MyInst_Start()) */
	
	CyPins_SetPinDriveMode(pins[0], PIN_DM_STRONG);
	CyPins_SetPinDriveMode(pins[1], PIN_DM_STRONG);
	CyPins_SetPinDriveMode(pins[2], PIN_DM_STRONG);
	CyPins_SetPinDriveMode(pins[3], PIN_DM_STRONG);
	
	
	CyPins_ClearPin(pins[0]);
	CyPins_ClearPin(pins[1]);
	CyPins_ClearPin(pins[2]);
	CyPins_ClearPin(pins[3]);
	
    /* CyGlobalIntEnable; */ /* Uncomment this line to enable global interrupts. */
    for(;;)
    {
	 
		position += turnFullForw(steps,Freq, mode);
		/*
		
		CyDelay(500);
		position += turnFullRev(steps,Freq, mode);
		stepstate = 0;
		
		CyDelay(500);
		
		position += turnFullRev(steps, Freq, mode);
		stepstate = 0;
		CyDelay(500);*/
	}
		
}


//////////////////////////////////////////

int turnWaveForw(int steps_, float freq_, int mode_)
{
	stepstate = 0;
	if(Last == 0)
	 ++spolecase;
	
	while(stepstate <= steps_)
	{
			if(mode_ == 1)   // Mode 1 her acceleres motoren op
				{
					if(stepup < freq_)
					{
						++stepup;
						delay = (1/stepup)*1000;
						
					}
				}
			else
				{
					delay = (1/freq_)*1000;
				}
			if(spolecase > 3 )
				spolecase = 0;
			if(spolecase < 0)
				spolecase = 3;
			
			switch(spolecase)
			{
				case 0:
				{
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 1:
				{
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					
				}
				break;
				case 2:
				{
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
				}
				break;
				case 3:
				{
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					
				}
				break;
			
			}
			++spolecase;
			CyDelay(delay);
			++stepstate;
	}
	stepup = 0;
	Last = 0;
	return (--stepstate);
}

int turnWaveRev(int steps_, float freq_, int mode_)
{
	stepstate = 0;
	if(Last == 1)
	 ++spolecase;
	
	while(stepstate <= steps_)
	{
			if(mode_ == 1)   // Mode 1 her acceleres motoren op
				{
					if(stepup < freq_)
					{
						++stepup;
						delay = (1/stepup)*1000;
						
					}
				}
			else
				{
					delay = (1/freq_)*1000;
				}
			if(spolecase > 3 )
				spolecase = 0;
			if(spolecase < 0)
				spolecase = 3;
			
			switch(spolecase)
			{
				case 0:
				{
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					
				}
				break;
				case 1:
				{
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 2:
				{
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					
				}
				break;
				case 3:
				{
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
				}
				break;
				
			
			}
			--spolecase;
			CyDelay(delay);
			++stepstate;
	}
	stepup = 0;
	Last = 1;
	return -(--stepstate);
}

int turnFullForw(int steps_, float freq_, int mode_)
{
	stepstate = 0;
	if(Last == 0)
	 ++spolecase;
	
	while(stepstate <= steps_)
	{
			if(mode_ == 1)   // Mode 1 her acceleres motoren op
				{
					if(stepup < freq_)
					{
						stepup +=freq_/5000;
						//++stepup;
						delay = (1/stepup)*1000;
						
					}
				}
			else
				{
					delay = (1/freq_)*1000;
				}
			if(spolecase > 3 )
				spolecase = 0;
			if(spolecase < 0)
				spolecase = 3;
			
			switch(spolecase)
			{
				case 0:
				{
					CyPins_SetPin(pins[0]);
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 1:
				{
					CyPins_SetPin(pins[3]);
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					
				}
				break;
				case 2:
				{
					CyPins_SetPin(pins[2]);
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
				}
				break;
				case 3:
				{
					CyPins_SetPin(pins[1]);
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					
				}
				break;
			
			}
			++spolecase;
			CyDelay(delay);
			++stepstate;
	}
	stepup = 0;
	//stepstate = stepstate-1;
	Last = 0;
	return (--stepstate);
}

int turnFullRev(int steps_, float freq_, int mode_)
{
	stepstate = 0;
	if(Last == 1)
	 ++spolecase;
	
	while(stepstate <= steps_)
	{
			if(mode_ == 1)   // Mode 1 her acceleres motoren op
				{
					if(stepup < freq_)
					{
						++stepup;
						delay = (1/stepup)*1000;
						
					}
				}
			else
				{
					delay = (1/freq_)*1000;
				}
			if(spolecase < 0 )
				spolecase = 3;
			if(spolecase > 3)
				spolecase = 0;
			
			switch(spolecase)
			{
				case 0:
				{
					CyPins_SetPin(pins[1]);
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					
				}
				break;
				case 1:
				{
					CyPins_SetPin(pins[0]);
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 2:
				{
					CyPins_SetPin(pins[3]);
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
				}
				break;
				case 3:
				{
					CyPins_SetPin(pins[2]);
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					
				}
				break;
			
			}
			--spolecase;
			CyDelay(delay);
			++stepstate;
	}
	//stepstate = stepstate-1;
	Last = 1;
	stepup = 0;
	return -(--stepstate);
} 

int turnHalfForw(int steps_, float freq_, int mode_)
{
	stepstate = 0;
	if(Last == 0)
	 ++spolecase;
	while(stepstate < steps_)
	{
			if(mode_ == 1)   // Mode 1 her acceleres motoren op
				{
					if(stepup < freq_)
					{
						++stepup;
						delay = (1/stepup)*1000;
						
					}
				}
			else
				{
					delay = (1/freq_)*1000;
				}
			if(spolecaseHalf > 7 )
				spolecaseHalf = 0;
			
			switch(spolecaseHalf)
			{
				case 0:
				{
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 1:
				{
					CyPins_SetPin(pins[3]);
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
				}
				break;
				case 2:
				{
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					
				}
				break;
				case 3:
				{
					CyPins_SetPin(pins[2]);
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					
				}
				break;
				case 4:
				{
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					
				}
				break;
				case 5:
				{
					CyPins_SetPin(pins[1]);
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
				}
				break;
				case 6:
				{
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					
				}
				break;
				case 7:
				{
					CyPins_SetPin(pins[0]);
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
			
			}
			++spolecaseHalf;
			CyDelay(delay);
			++stepstate;
	}
	stepup = 0;
	Last = 0;
	return (--stepstate);
}

int turnHalfRev(int steps_, float freq_, int mode_)
{
	stepstate = 0;
	if(Last == 1)
	 ++spolecase;
	
	while(stepstate < steps_)
	{
			if(mode_ == 1)   // Mode 1 her acceleres motoren op
				{
					if(stepup < freq_)
					{
						++stepup;
						delay = (1/stepup)*1000;
						
					}
				}
			else
				{
					delay = (1/freq_)*1000;
				}
			if(spolecaseHalf < 0 )
				spolecaseHalf = 7;
			
			switch(spolecaseHalf)
			{
				case 0:
				{
					CyPins_SetPin(pins[0]);
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 1:
				{
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 2:
				{
					CyPins_SetPin(pins[3]);
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
				}
				break;
				case 3:
				{
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					
				}
				break;
				case 4:
				{
					CyPins_SetPin(pins[2]);
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					
				}
				break;
				case 5:
				{
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					
				}
				break;
				case 6:
				{
					CyPins_SetPin(pins[1]);
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
				}
				break;
				case 7:
				{
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					
				}
				break;
			
			
			}
			--spolecaseHalf;
			CyDelay(delay);
			++stepstate;
	}
	stepup = 0;
	Last = 1;	 
	return -(--stepstate);
}